;;;; -*- Mode: Lisp; Package: :xlui-user -*-
;;;;
;;;; transformation.lisp
;;;;

(in-package :lib.gl)

(export '(make-transform transform transform-r transform-s transform-b
	  set-rotation set-scale set-translation axis-rotation
	  xyz-rotate translate scale-matrix homogeneous-matrix)) 

(defstruct transform
  (r (rotation-matrix-to-quat (identity-matrix 3)))
  (s (make-vector 1.0 1.0 1.0))
  (b (zero-vector 3))
  (hm (zero-matrix 4 4))
  (cached nil)
  (ident-p t)
  (uniform-scale-p t)
  (rs-matrix-p nil))

(defun set-rotation (m r)
  (setf (transform-cached m) nil
	(transform-r m) r))

(defun set-scale (m s)
  (setf (transform-cached m) nil
	(transform-s m) s))

(defun set-translation (m b)
  (setf (transform-cached m) nil
	(transform-b m) b))

(defun axis-rotation (m direction rot &optional (in-radians t))
  (when (not in-radians)
    (setf rot (* 0.5 rot PI)))
  ;; Should check that direction is a unit vector
  (setf (transform-cached m) nil
	(transform-r m) (quat-mult (transform-r m) (axis-angle-to-quat direction rot))))

(defun %axis-rotation-matrix (m direction rot &optional (in-radians t))
  (when (not in-radians)
    (setf rot (* 0.5 rot PI)))
  (let* ((c (cos rot))
	 (s (sin rot))
	 (x (x direction)) (y (y direction)) (z (z direction))
	 (xx (* x x)) (yy (* y y)) (zz (* z z))
	 (row1 (list (+ xx (* (- 1 xx) c))       (- (* x y (- 1 c)) (* z s)) (+ (* x z (- 1 c)) (* y s))))
	 (row2 (list (+ (* x y (- 1 c)) (* z s)) (+ yy (* (- 1 yy) c))        (+ (* y z (- 1 c)) (* x s))))
	 (row3 (list (+ (* x z (- 1 c)) (* y s)) (+ (* y z (- 1 c)) (* x s)) (+ zz (* (- 1 zz) c)))))
    (setf (transform-cached m) nil
	  (transform-r m) (rotation-matrix-to-quat (m* (%matrix (list row1 row2 row3)) (transform-r m))))))
#||#

(defun xyz-rotate (m rot-x rot-y rot-z &optional (in-radians t))
  (when (not in-radians)
    (setf rot-x (* 0.5 rot-x +PI/180+)
	  rot-y (* 0.5 rot-y +PI/180+)
	  rot-z (* 0.5 rot-z +PI/180+)))
  (setf (transform-cached m) nil
	(transform-r m) (nquat-mult (transform-r m) (euler-to-quat rot-x rot-y rot-z))))

(defun translate (m dx dy dz)
  (setf (transform-cached m) nil)
  (incf (x (transform-b m)) dx)
  (incf (y (transform-b m)) dy)
  (incf (z (transform-b m)) dz))

(defun scale-matrix (scale-vector &optional (homogeneous nil))
  (let ((m (if homogeneous 
	       (identity-matrix 4)
	       (identity-matrix 3))))
    (setf (mref m 0 0) (x scale-vector)
	  (mref m 1 1) (y scale-vector)
	  (mref m 2 2) (z scale-vector))
    m))

(defun homogeneous-matrix (transform)
  (if (transform-cached transform)
      (transform-hm transform)
      (let ((hm (lib.gl:quat-to-homogeneous-matrix (transform-r transform) (transform-hm transform)))
	    (b (transform-b transform))
	    (s (scale-matrix (transform-s transform) t)))
	(setf (mref hm 0 3) (x b)
	      (mref hm 1 3) (y b)
	      (mref hm 2 3) (z b))
	(if (not (transform-rs-matrix-p transform))
	    (setf hm (m* hm s)))
	(setf (transform-cached transform) t)
	hm)))

(defun %uniform-scale-product (m1 m2)
  (let ((r1 (transform-r m1))
	(b1 (transform-b m1))
	(s1 (transform-s m1))
	(r2 (transform-r m2))
	(b2 (transform-b m2))
	(s2 (transform-s m2)))
    (let* ((s1 (mref s1 0))
	   (s2 (mref s2 0))
	   (s1s2 (* s1 s2)))
      (make-transform :r (m* s1s2 (quat-mult r1 r2))
		      :b (m+ (m* (quat-to-rotation-matrix r2) s2 b2) b1)
		      :s (make-vector s1s2 s1s2 s1s2)
		      :ident-p nil
		      :uniform-scale-p t
		      :rs-matrix-p t))))

(defun tproduct (m1 m2)
  "Returns the product of two transforms."
  ;; TODO be smarter handling when both transforms are RS matrices
  (cond ((transform-ident-p m1) (copy-transform m2))
	((transform-ident-p m2) (copy-transform m1))
	((and (transform-uniform-scale-p m1) (transform-uniform-scale-p m2))
	 (%uniform-scale-product m1 m2))
	(t (let* ((b1 (transform-b m1))
		  (s1 (scale-matrix (transform-s m1)))
		  (r1 (quat-to-rotation-matrix (transform-r m1)))
		  (r2 (transform-r m2))
		  (b2 (transform-b m2))
		  (s2 (scale-matrix (transform-s m2)))
		  (r2 (quat-to-rotation-matrix (transform-r m2)))
		  (rn (m* r2 s2 r1 s1))
		  (tn (m+ (m* r2 s2 b2) b1)))
	     (make-transform :r (rotation-matrix-to-quat rn)
			     :b tn
			     :ident-p nil
			     :uniform-scale-p nil
			     :rs-matrix-p t)))))


(defmethod apply-forward (m (v vector) &key (reuse nil))
;;(format t "Apply-forward:  ident=~A  rs-matrix=~A~%" ident-p rs-matrix-p)
;;    (if ident-p 
;;	v
  (let ((r (quat-to-rotation-matrix (transform-r m)))
	(s (transform-s m))
	(b (transform-b m))
	(nv (if reuse v (zero-vector 3))))
    (cond ((transform-rs-matrix-p m) 
	   (setf (x nv) (* (x v) (x s))
		 (y nv) (* (y v) (y s))
		 (z nv) (* (z v) (z s)))
	   (m+ (m* r nv) b))
	  (t (m+ (m* r (m* (x s) v)) b))))) ;; Uniform scale

;;;; Testing, performance

(defun test-rotate-1 ()
  (let ((m (make-transform)))
    (dotimes (i 10000)
      (xyz-rotate m 0.1 0.1 0.1 t))))

(defun test-rotate-2 ()
  (let ((m (make-transform))
	(rs (zero-matrix 4)))
    (dotimes (i 10000)
      (lib.gl:quat-to-homogeneous-matrix (transform-r m) rs))))

(defun test-rotate-performance ()
  (let ((m (make-transform)))
    (dotimes (i 10000 m)
      (xyz-rotate m 0.1 0.0 0.0)
      (transform-r m))))

(defun test-homogeneous ()
  (let ((m (make-transform)))
    (dotimes (i 10)
      (xyz-rotate m 0.1 0.0 0.0)
      (translate m 0.1 0.1 0.1))
    m))

(defun test-homogeneous-performance-1 ()
  (let ((m (test-homogeneous)))
    (time (dotimes (i 10000 m)
	    (homogeneous-matrix m)))))

(defun test-homogeneous-performance-2 ()
  (let ((m (test-homogeneous))
	(rs (zero-matrix 4)))
    (time (dotimes (i 10000)
	    (xyz-rotate m 0.1 0.1 0.1 t)
	    (homogeneous-matrix m)))))

